#include "joint.h"
#include "DM4340.h"
#include "math.h"
#include "dbus.h"
#include "main.h"
#include "gravity_cal.h"
#include <stdlib.h>

#define PI 3.1415926535

#define BACKARM_LIM -2.35f
#define FOREARM_LIM -2.35f

joint_dataTypeDef joint_data={0};

void joint_init()
{
    DM_MotorGroup_Init(&hcan1, DM4340, 3, mit_mode);
    save_pos_zero(&hcan1, 0x05, mit_mode);
    save_pos_zero(&hcan1, 0x04, mit_mode);
    HAL_Delay(20);
    DM_Motor_SoftStart(DM4340, 3, 3);
}

void joint_callback()
{
    joint_data.tgt_angle[1] += ((float)dbus_ctrl_data.channel.CH[0] - 1024.0f)*0.001/680.0f;
    joint_data.tgt_angle[2] += ((float)dbus_ctrl_data.channel.CH[1] - 1024.0f)*0.008/680.0f;

    if (joint_data.tgt_angle[1] < BACKARM_LIM) joint_data.tgt_angle[1] = BACKARM_LIM;
    if (joint_data.tgt_angle[1] > 0.0f) joint_data.tgt_angle[1] = 0.0f;

    if (joint_data.tgt_angle[2] < FOREARM_LIM) joint_data.tgt_angle[2] = FOREARM_LIM;
    if (joint_data.tgt_angle[2] > 0.0f) joint_data.tgt_angle[2] = 0.0f;

}

void joint_exe()
{
   joint_callback();
   mit_ctrl(DM4340, 0x05, joint_data.tgt_angle[1], 0, 30.0, 1.1, 0);
   mit_ctrl(DM4340, 0x04, joint_data.tgt_angle[2], 0, 4.3, 1, 0);
}
